/*!
 * @file rt_spi.h
 * @brief SPI communication to spine board
 */

#ifndef _rt_spi
#define _rt_spi

#ifdef linux

#include <sys/ioctl.h>//Needed for SPI port
#include <fcntl.h>    //Needed for SPI port

// incredibly obscure bug in SPI_IOC_MESSAGE macro is fixed by this
#ifdef __cplusplus /* If this is a C++ compiler, use C linkage */
extern "C"
{
#endif

#include <linux/spi/spidev.h>

#ifdef __cplusplus /* If this is a C++ compiler, use C linkage */
}
#endif

#include <spi_command_t.hpp>
#include <spi_data_t.hpp>
#include <spi_torque_t.hpp>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>//Needed for SPI port

#define K_EXPECTED_COMMAND_SIZE 256
#define K_WORDS_PER_MESSAGE     66
#define K_EXPECTED_DATA_SIZE    260// 116
#define K_KNEE_OFFSET_POS       4.35f

///////
#define FRONT_ABAD_OFFSET 0.3f//0.385f
#define FRONT_HIP_OFFSET  1.12f//1.0f
#define FRONT_KNEE_OFFSET 2.64f//2.7f// 4.19f
#define BACK_ABAD_OFFSET  0.3f//0.385f
#define BACK_HIP_OFFSET   1.12//1.0f
#define BACK_KNEE_OFFSET  2.64f//2.7f// 4.19f
#define KNEE_K            1.f // 0.6429f
///////

#define BYTE_TO_BINARY_PATTERN "%c%c%c%c%c%c%c%c"
#define BYTE_TO_BINARY(byte)                                      \
    (byte & 0x80 ? '1' : '0'), (byte & 0x40 ? '1' : '0'),         \
            (byte & 0x20 ? '1' : '0'), (byte & 0x10 ? '1' : '0'), \
            (byte & 0x08 ? '1' : '0'), (byte & 0x04 ? '1' : '0'), \
            (byte & 0x02 ? '1' : '0'), (byte & 0x01 ? '1' : '0')

void init_spi();

void spi_send_receive(spi_command_t* command, spi_data_t* data);
void spi_driver_run();

spi_data_t*    get_spi_data();
spi_command_t* get_spi_command();

/*!
 * SPI command message
 */
typedef struct
{
    float   q_des_abad[2];
    float   q_des_hip[2];
    float   q_des_knee[2];
    float   qd_des_abad[2];
    float   qd_des_hip[2];
    float   qd_des_knee[2];
    float   kp_abad[2];
    float   kp_hip[2];
    float   kp_knee[2];
    float   kd_abad[2];
    float   kd_hip[2];
    float   kd_knee[2];
    float   tau_abad_ff[2];
    float   tau_hip_ff[2];
    float   tau_knee_ff[2];
    int32_t flags[2];
    int32_t checksum;

} spine_cmd_t;

/*!
 * SPI data message
 */
typedef struct
{
    float   q_abad[2];
    float   q_hip[2];
    float   q_knee[2];
    float   qd_abad[2];
    float   qd_hip[2];
    float   qd_knee[2];
    float   tau_abad[2];
    float   tau_hip[2];
    float   tau_knee[2];
    float   temperature_abad[2];
    float   temperature_hip[2];
    float   temperature_knee[2];
    int32_t status_abad[2];
    int32_t status_hip[2];
    int32_t status_knee[2];
    int32_t flags[2];
    int32_t checksum;

} spine_data_t;

// 关节命令结构体：5*4=20字节
typedef struct
{
    float a; // 角度 rad
    float w; // 角速度 rad/s
    float t; // 力矩 N*m
    float kp;// 比例系数
    float kd;// 微分系数
} JointCommandStruct;

// 单腿命令结构体：20*3+4=64字节
typedef struct
{
    JointCommandStruct abad;// 髋关节内外关节
    JointCommandStruct hip; // 髋关节屈伸关节
    JointCommandStruct knee;// 膝关节关节
    union
    {
        int32_t flag;// 标志位
        struct
        {
            int32_t enable     : 1; // 使能
            int32_t reserved   : 1; // 保留
            int32_t setZero    : 1; // 设置零位
            int32_t clearFault : 1; // 清除故障
            int32_t reserved1  : 28;// 保留
        };
    };
} LegCommandStruct;

// SPINE命令数据结构体：前双腿或后双腿 64*2+4=132字节
typedef struct
{
    LegCommandStruct left;    // 左腿
    LegCommandStruct right;   // 右腿
    int32_t          checksum;// 校验和
} SpineCommandStruct;

// 电机模式枚举
typedef enum
{
    _RESET_MODE,      // 复位
    _CALIBRATION_MODE,// 校准
    _RUN_MODE,        // 运行
} MotorModeEnum;

// 电机故障码2联合体
typedef union
{
    uint8_t code;// 故障码
    struct
    {
        uint8_t underVoltage         : 1;// [0] 欠压
        uint8_t overCurrent          : 1;// [1] 过流
        uint8_t overTemperature      : 1;// [2] 过温
        uint8_t magneticEncoderFault : 1;// [3] 磁编码器故障
        uint8_t hallEncoderFault     : 1;// [4] 霍尔编码器故障
        uint8_t notCalibrated        : 1;// [5] 未标定
        uint8_t reserved             : 2;// 保留
    };
} FaultCode2Union;

// 电机故障码21联合体
typedef union
{
    int32_t code;// 故障码
    struct
    {
        int32_t over80Celsius   : 1; // [0] 超过80°C
        int32_t driverChipFault : 1; // [1] 驱动芯片故障
        int32_t underVoltage    : 1; // [2] 欠压
        int32_t overVoltage     : 1; // [3] 过压
        int32_t overCurrentB    : 1; // [4] 过流_B相
        int32_t overCurrentC    : 1; // [5] 过流_C相
        int32_t reserved        : 1; // 保留
        int32_t notCalibrated   : 1; // [7] 未标定
        int32_t overload        : 8; // [15:8] 过载
        int32_t overCurrentA    : 1; // [16] 过流_A相
        int32_t over75Celsius   : 1; // [17] 超过75°C
        int32_t reserved1       : 14;// 保留
    };
} FaultCode21Union;

// 关节数据结构体：5*4=20字节
typedef struct
{
    float   a;          // 角度 rad
    float   w;          // 角速度 rad/s
    float   t;          // 力矩 N*m
    float   temperature;// 温度 °C
    int32_t status;     // 状态：[31:30]MotorModeEnum，[29:24]FaultCode2Union, [17:0]FaultCode21Union
} JointDataStruct;

// 单腿数据结构体：20*3+4=64字节
typedef struct
{
    JointDataStruct abad;// 髋关节内外关节
    JointDataStruct hip; // 髋关节屈伸关节
    JointDataStruct knee;// 膝关节关节
    union
    {
        int32_t flag;// 标志位
        struct
        {
            int32_t cmdCheckFail : 1; // 命令校验失败
            int32_t isEstop      : 1; // 急停
            int32_t isReady      : 1; // 准备就绪
            int32_t reserved     : 29;// 保留
        };
    };
} LegDataStruct;

// SPINE数据结构体：前双腿或后双腿 64*2+4=132字节
typedef struct
{
    LegDataStruct left;    // 左腿
    LegDataStruct right;   // 右腿
    int32_t       checksum;// 校验和
} SpineDataStruct;

#endif// END of #ifdef linux

#endif
